#include <math.h>

#include "identificacao.h"
#include "mainwindow.h"

identificacao* identificacao::instance = NULL;

identificacao::identificacao() : tempoIdent(0), timePast(0), timeNow(0), tempoAux(0), factor(0),
                                 cycle(3), identifying(false)
{
    running = false;
    faixas.clear();
    TimePoints.clear();
    PVPoints.clear();
    CVPoints.clear();

    ganho = 0;
    constTempo = 0;
    tempoMorto = 0;
    fatorIncont = 0;
}

identificacao::~identificacao()
{
    instance = NULL;
    faixas.clear();
    TimePoints.clear();
    PVPoints.clear();
    CVPoints.clear();
}

identificacao* identificacao::getInstance()
{
    if (instance == NULL)
        instance = new identificacao();
    return instance;
}

void identificacao::calcStep() {
    double deltaTime;
    double PV = pid::y;
    double CV = pid::controlSignal;
    double timePID = pid::time;

    tempoAux -= 0.1;
    if(tempoAux <= 0){
        if(running == true){
            if (identifying) {
                Ciclos aux = {TimePoints, PVPoints, CVPoints};
                faixas.push_back(aux);
                cycle++;
                endOfCycle();
            }
            else
                refreshCycle(cycle);

            if (cycle >= 3) {
                identifying = false;
                running = false;
                calcFODT();
                emit endIdentify();
            }
        }
        return;
    }

    if (identifying) {
        deltaTime = timePID - timePast;
        timeNow += deltaTime;

        TimePoints.push_back(timeNow);
        PVPoints.push_back(PV);
        CVPoints.push_back(CV);

        timePast = timePID;
    }
}

// Mude valor do ciclo entre 1, 2 ou 3 para definir quantas
// etapas de degrau serao dadas durante a identificacao
void identificacao::setTime(int time) {
    faixas.clear();
    TimePoints.clear();
    PVPoints.clear();
    CVPoints.clear();
    tempoIdent = time;
    tempoAux = time;
    timePast = pid::time;
    timeNow = 0;

    cycle = 0;

    running = true;
    identifying = true;

    TimePoints.push_back(timeNow);
    PVPoints.push_back(pid::y);
    CVPoints.push_back(pid::controlSignal);

    factor = 1.2;
    pid::controlSignal *= factor;
    emit changeSetPoint(factor);
}

void identificacao::cancel() {
    running = false;
    identifying = false;
    tempoAux = 0;
    pid::controlSignal *= 1/factor;
    emit changeSetPoint(1/factor);
}

void identificacao::endOfCycle() {
    TimePoints.clear();
    PVPoints.clear();
    CVPoints.clear();
    tempoAux = tempoIdent;
    timePast = pid::time;
    timeNow = 0;
    identifying = false;

    pid::controlSignal *= 1/factor;
    emit changeSetPoint(1/factor);
}

void identificacao::refreshCycle(int cycle) {
    tempoAux = tempoIdent;
    timePast = pid::time;
    timeNow = 0;
    identifying = true;

    TimePoints.push_back(timeNow);
    PVPoints.push_back(pid::y);
    CVPoints.push_back(pid::controlSignal);

    if (cycle == 1) {
        factor = 0.8;
        pid::controlSignal *= factor;
        emit changeSetPoint(factor);
    }
    else if (cycle == 2){
        factor = 1.3;
        pid::controlSignal *= factor;
        emit changeSetPoint(factor);
    }
}

void identificacao::calcFODT() {
    printf("TamFaixa: %i\n", faixas.size());

    ganho = 0;
    constTempo = 0;
    tempoMorto = 0;

    vvector coef;

    std::vector<double> x;
    std::vector<double> y;
    std::vector<double> fx;
    std::vector<double> fx2;
    double inflexao;
    double m;
    double b;

    double deltaPV;
    double deltaCV;
    double faixasSize = faixas.size();
    for (int i = 0; i < faixasSize; i++) {
        x.clear();
        y.clear();
        fx.clear();
        fx2.clear();
        inflexao = -1;
        m = 0;
        b = 0;

        printf("%i -- TamTime: %i front: %f back: %f\n", i, faixas[i].TimePoints.size(), faixas[i].TimePoints.front(), faixas[i].TimePoints.back());
        printf("%i -- TamPV: %i front: %f back: %f\n", i, faixas[i].PVPoints.size(), faixas[i].PVPoints.front(), faixas[i].PVPoints.back());
        printf("%i -- TamCV: %i front: %f back: %f\n", i, faixas[i].CVPoints.size(), faixas[i].CVPoints.front(), faixas[i].CVPoints.back());

        double backPV = faixas[i].PVPoints.back();
        double frontPV = faixas[i].PVPoints.front();
        double backCV = faixas[i].CVPoints.back();
        double frontCV = faixas[i].CVPoints.front();

        double TMAux = 0;
        int indexCT;

        deltaPV = backPV - frontPV;
        deltaCV = backCV - frontCV;

        printf("%i -- ganho: %f\n", i, deltaPV/deltaCV);
        ganho += deltaPV/deltaCV;

        coef = ajpoly(faixas[i].TimePoints, faixas[i].PVPoints, 3);

        std::cout << "Coeficiientes " << std::endl;
        std::cout << coef;
        std::cout << std::endl;

        x = faixas[i].TimePoints;

        y = gerarY(coef,x);

        fx = mdf(y,0.1);
        fx2 = mdf(fx,0.1);

        double aux = frontPV + (0.632*deltaPV);

        if (backPV > frontPV) {
            inflexao = findIndex(fx2,0,0);
            indexCT = findIndex(faixas[i].PVPoints,true,aux);
        }
        else {
            inflexao = findIndex(fx2,1,0);
            indexCT = findIndex(faixas[i].PVPoints,false,aux);
        }

        inflexao < 2 ? 2 : inflexao;

        m = (y[inflexao+1]-y[inflexao-1])/(x[inflexao+1]-x[inflexao-1]);
        b = y[inflexao]-m*x[inflexao];

        TMAux = (frontPV-b)/m;
        printf("%i -- TMAux: %f\n",i, TMAux);

        tempoMorto += TMAux;

        if (indexCT != -1) {
            printf("%i -- ConstTempo: %f\n\n",i, faixas[i].TimePoints[indexCT]-TMAux);
            constTempo += faixas[i].TimePoints[indexCT]-TMAux;
        }
        else
            printf("Falha para encotrar constante de tempo\n");
    }

    ganho /= faixasSize;
    tempoMorto /= faixasSize;
    constTempo /= faixasSize;

    fatorIncont = tempoMorto/constTempo;

    printf("GANHO: %f\n", ganho);
    printf("TempoMorto: %f\n", tempoMorto);
    printf("Constante de tempo: %f\n", constTempo);
    printf("Constante de tempo: %f\n", constTempo);
}

int identificacao::findIndex(vector<double> pvVector, bool crescente, double lim) {
    int index = -1;

    for(int i = 0; i < pvVector.size(); i++) {
        if (crescente) {
            if (pvVector[i]>lim) {
                index = i;
                break;
            }
        }
        else {
            if (pvVector[i]<lim) {
                index = i;
                break;
            }
        }
    }
    return index;
}

std::vector<double> identificacao::mypow(std::vector<double> x, int expoente) {
    std::vector<double> powX;
    int size = x.size();
    for (int i = 0; i < size; i++) {
        powX.push_back(pow(x[i],expoente));
    }

    return powX;
}

double identificacao::mysum(std::vector<double> x) {
    int size = x.size();
    double sum = 0;
    for (int i = 0; i < size; i++) {
        sum+=x[i];
    }
    return sum;
}

std::vector<double> identificacao::prodInterno(std::vector<double> x,std::vector<double> y) {
    int sizeX = x.size();
    int sizeY = y.size();
    std::vector<double> xy;

    if (sizeX != sizeY) {
        printf("Tamanhos dos vetores incompativeis para fazer um produto interno\n");
    }
    else {
        for (int i = 0; i < sizeX; i++) {
            xy.push_back(y[i]*x[i]);
        }
    }
    return xy;
}

std::vector<double> identificacao::gerarY(vvector c, std::vector<double> x) {
    std::vector<double> y;
    int size = x.size();
    double aux = 0;
    for (int i = 0; i < size; i++) {
         //aux = c[0] + c[1]*x[i] + c[2]*pow(x[i],2) + c[3]*pow(x[i],3) +
         //      c[4]*pow(x[i],4) + c[5]*pow(x[i],5) + c[6]*pow(x[i],6);
        aux = c[0] + c[1]*x[i] + c[2]*pow(x[i],2) + c[3]*pow(x[i],3);
        y.push_back(aux);
    }
    return y;
}

fvector identificacao::ajpoly(std::vector<double> x, std::vector<double> y, int ordem) {
    int n = ordem+1;
    fmatrix A(n);
    fvector b(n);

    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j++) {
            A[i][j] = mysum(mypow(x,i+j));
        }
    }

    for (int i = 0; i < n; i++) {
        b[i] = mysum(prodInterno(mypow(x,i),y));
    }

    fvector coef = A.solve(b);
    return coef;
}

std::vector<double> identificacao::mdf(std::vector<double> f, double h) {
    int size = f.size();
    std::vector<double> fx;

    double aux;
    for (int i = 0; i < size; i++) {
        if (i != 0 && size-1) {
            aux = (f[i+1]-f[i-1])/(2*h);
            fx.push_back(aux);
        }
        else
            fx.push_back(0);
    }

    return fx;
}

void identificacao::printVector(std::vector<double> x) {
    std::vector<double>::iterator it;
    for(it = x.begin(); it < x.end(); it++) {
        printf("%.4f ",*it);
    }
    printf("\n");
}
